// Background average sample code done with averages and done with codebooks
// (adapted from the OpenCV book sample)
// 
// NOTE: To get the keyboard to work, you *have* to have one of the video windows be active
//       and NOT the consule window.
//
// Gary Bradski Oct 3, 2008.
// 
/* *************** License:**************************
   Oct. 3, 2008
   Right to use this code in any way you want without warrenty, support or any guarentee of it working.

   BOOK: It would be nice if you cited it:
   Learning OpenCV: Computer Vision with the OpenCV Library
     by Gary Bradski and Adrian Kaehler
     Published by O'Reilly Media, October 3, 2008
 
   AVAILABLE AT: 
     http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
     Or: http://oreilly.com/catalog/9780596516130/
     ISBN-10: 0596516134 or: ISBN-13: 978-0596516130    
************************************************** */

#include "stdafx.h"
#include "cvaux.h"
#include "cxmisc.h"
#include "highgui.h"
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <time.h>

//VARIABLES for CODEBOOK METHOD:
CvBGCodeBookModel* model = 0;
const int NCHANNELS = 3;
bool ch[NCHANNELS]={true,true,true}; // This sets what channels should be adjusted for background bounds

void help(void)
{
    printf("\nLearn background and find foreground using simple average and average difference learning method:\n"
        "\nUSAGE:\nbgfg_codebook [--nframes=300] [movie filename, else from camera]\n"
        "***Keep the focus on the video windows, NOT the consol***\n\n"
        "INTERACTIVE PARAMETERS:\n"
        "\tESC,q,Q  - quit the program\n"
        "\th	- print this help\n"
        "\tp	- pause toggle\n"
        "\ts	- single step\n"
        "\tr	- run mode (single step off)\n"
        "=== AVG PARAMS ===\n"
        "\t-    - bump high threshold UP by 0.25\n"
        "\t=    - bump high threshold DOWN by 0.25\n"
        "\t[    - bump low threshold UP by 0.25\n"
        "\t]    - bump low threshold DOWN by 0.25\n"
        "=== CODEBOOK PARAMS ===\n"
        "\ty,u,v- only adjust channel 0(y) or 1(u) or 2(v) respectively\n"
        "\ta	- adjust all 3 channels at once\n"
        "\tb	- adjust both 2 and 3 at once\n"
        "\ti,o	- bump upper threshold up,down by 1\n"
        "\tk,l	- bump lower threshold up,down by 1\n"
        "\tSPACE - reset the model\n"
        );
}

uchar getPixel(IplImage* img, int lin, int col, int channel)
{
	return ((uchar*)(img->imageData + img->widthStep*lin))[col+channel];
}

//
//USAGE:  ch9_background startFrameCollection# endFrameCollection# [movie filename, else from camera]
//If from AVI, then optionally add HighAvg, LowAvg, HighCB_Y LowCB_Y HighCB_U LowCB_U HighCB_V LowCB_V
//
int main(int argc, char** argv)
{
    const char* filename = 0;
	const char* title = "Let's play Simon!";
	const char* title2 = "Repeat the series of numbers I say by holding up fingers!";
	const char* numbers[] = {"0", "1", "2", "3", "4"};
	char *list = (char *)malloc(sizeof(char)*100);
	char *theirList = (char*)malloc(sizeof(char)*100);
    IplImage* rawImage = 0, *yuvImage = 0; //yuvImage is for codebook method
    IplImage *ImaskCodeBook = 0,*ImaskCodeBookCC = 0;
    CvCapture* capture = 0;
	CvFont font;
	CvPoint thumbLoc;
	thumbLoc.x = 10;
	thumbLoc.y = 70;
	CvPoint fingerLoc;
	fingerLoc.x = 10;
	fingerLoc.y = 140;
	CvPoint fingerTextLoc;
	fingerTextLoc.x = 10;
	fingerTextLoc.y = 140;
	char *text = (char *)malloc(sizeof(char)*3);
	text[1] = 0;
	text[2] = 0;
	clock_t startTurn = 0; 
	clock_t lockTimer = 0;
	bool simonsTurn = false;
	bool fail = false;
	int index = 0;
	int turnNumber = -1;
	int curNumFingers = 0;
	uchar pix;
	int numFingers = 0;
	int prevNumFingers = 0;
	bool finger = false, thumb = false, prevThumb = false, possibleThumb = false;
	int count = 0;

    int c, n, nframes = 0;
    int nframesToLearnBG = 300;

    model = cvCreateBGCodeBookModel();
    
    //Set color thresholds to default values
    model->modMin[0] = 3;
    model->modMin[1] = model->modMin[2] = 3;
    model->modMax[0] = 10;
    model->modMax[1] = model->modMax[2] = 10;
    model->cbBounds[0] = model->cbBounds[1] = model->cbBounds[2] = 10;

    bool pause = false;
    bool singlestep = false;

	//initialize simon list
	for(int i = 0; i < 100; i++)
	{
		if(i > 0)
		{
			do {
				list[i] = (clock()*13)%4+1;
			} while(list[i] == list[i-1]);
		}
		else
			list[i] = (clock()*13)%4+1;
	}

    for( n = 1; n < argc; n++ )
    {
        static const char* nframesOpt = "--nframes=";
        if( strncmp(argv[n], nframesOpt, strlen(nframesOpt))==0 )
        {
            if( sscanf(argv[n] + strlen(nframesOpt), "%d", &nframesToLearnBG) == 0 )
            {
                help();
                return -1;
            }
        }
        else
            filename = argv[n];
    }

    if( !filename )
    {
        printf("Capture from camera\n");
        capture = cvCaptureFromCAM( 0 );
    }
    else
    {
        printf("Capture from file %s\n",filename);
        capture = cvCreateFileCapture( filename );
    }

    if( !capture )
    {
        printf( "Can not initialize video capturing\n\n" );
        help();
        return -1;
    }

    //MAIN PROCESSING LOOP:
    for(;;)
    {
        if( !pause )
        {
            rawImage = cvQueryFrame( capture );
            ++nframes;
            if(!rawImage) 
                break;
        }
        if( singlestep )
            pause = true;
        
        //First time:
        if( nframes == 1 && rawImage )
        {
            // CODEBOOK METHOD ALLOCATION
            yuvImage = cvCloneImage(rawImage);
            ImaskCodeBook = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
            ImaskCodeBookCC = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
            cvSet(ImaskCodeBook,cvScalar(255));
            
            cvNamedWindow( "Raw", 1 );
   //         cvNamedWindow( "ForegroundCodeBook",1);
      //      cvNamedWindow( "CodeBook_ConnectComp",1);
        }

        // If we've got an rawImage and are good to go:                
        if( rawImage )
        {
            cvCvtColor( rawImage, yuvImage, CV_BGR2YCrCb );//YUV For codebook method
            //This is where we build our background model
            if( !pause && nframes-1 < nframesToLearnBG  )
                cvBGCodeBookUpdate( model, yuvImage );

            if( nframes-1 == nframesToLearnBG  )
                cvBGCodeBookClearStale( model, model->t/2 );
            
            //Find the foreground if any
            if( nframes-1 >= nframesToLearnBG  )
            {
                // Find foreground by codebook method
                cvBGCodeBookDiff( model, yuvImage, ImaskCodeBook );
                // This part just to visualize bounding boxes and centers if desired
                cvCopy(ImaskCodeBook,ImaskCodeBookCC);	
                cvSegmentFGMask( ImaskCodeBookCC );
            }
            //Display

			cvRectangle(rawImage, cvPoint(ImaskCodeBookCC->width / 4, ImaskCodeBookCC->height / 4), cvPoint(ImaskCodeBookCC->width*3/4, ImaskCodeBookCC->height*3/4),  cvScalar(255, 255, 255, 0), 2, 0, 0);

			//Count horizontal white pixel groups in each line of ImaskCodeBookCC
			//Must have at least 10 pixels between fingers
			count = 0;
			finger = false;
			thumb = false;
			prevThumb = false;
			possibleThumb = false;
			numFingers = 0;
			curNumFingers = 0;
			pix = 0;
			if( !pause && nframes > nframesToLearnBG  )
			{
				for(int j = ImaskCodeBookCC->height/4; j < (ImaskCodeBookCC->height*3)/4; j++)
				{
					for(int i = ImaskCodeBookCC->width/4; i < (ImaskCodeBookCC->width*3)/4; i++)
					{
						pix = getPixel(ImaskCodeBookCC, j, i, 0);
						if(pix) //it's white.
						{
							if(!finger) 
							{
								finger = true;	
							}
							count++;

						}
						else
						{
							if(finger)
							{
								if(count > 10 && count < 76)
								{
									numFingers++;
									possibleThumb = false;
								}
								else if(count > 75 && numFingers == 1) // if thumb on left
								{
									thumb = true;
									numFingers = 0;
									possibleThumb = false;
								}
								else if(count > 75)
								{
									possibleThumb = true;
								}
								else if(possibleThumb && count > 10) //if thumb on right
								{
									thumb = true;
									possibleThumb = false;
								}
								finger = false;
							}
							count = 0;
						}
					}
					if(thumb)
					{
						prevThumb = true;
					}
					if(numFingers > curNumFingers)
					{
						curNumFingers = numFingers;
					}
					count = 0;
					numFingers = 0;
					thumb = false;
				}
				
			}
			if(curNumFingers > 4)
				curNumFingers = 5;
			
			if(curNumFingers != prevNumFingers)
			{
				prevNumFingers = curNumFingers;
				lockTimer = clock();
			}

			cvFlip(rawImage, rawImage, 1);

			cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 3, 3, 0.0, 5, 8);

//			if(prevThumb)
//			{
//				cvPutText(rawImage, "THUMB = true", thumbLoc, &font, CV_RGB(255, 0, 0));
//			}
//			else
//			{
//				cvPutText(rawImage, "THUMB = false", thumbLoc, &font, CV_RGB(255, 0, 0));
//			}
			clock_t curTime = clock();
			if(prevNumFingers == 0)
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[0], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, numbers[0], fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			else if(prevNumFingers == 1)
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[1], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, numbers[1], fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			else if(prevNumFingers == 2)
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[2], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, numbers[2], fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			else if(prevNumFingers == 3)
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[3], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, numbers[3], fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			else if(prevNumFingers == 4)
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[4], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, numbers[4], fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			else
			{
				if(curTime - lockTimer > 500)
				{
					cvPutText(rawImage, numbers[5], fingerTextLoc, &font, CV_RGB(0, 255, 0));
				}
				else
					cvPutText(rawImage, "5", fingerTextLoc, &font, CV_RGB(255, 255, 255));
			}
			if(simonsTurn)
			{
				if(curTime - startTurn - 1000 < 0)
				{
					cvPutText(rawImage, "READY?", thumbLoc, &font, CV_RGB(255, 0, 0));
				}
				else if(curTime - startTurn - 2000 < 0)
				{
					cvPutText(rawImage, "GO!", thumbLoc, &font, CV_RGB(0, 255, 0));
				}
				else
				{
					index = (curTime - startTurn - 2000)/500;
					cvPutText(rawImage, numbers[list[index]], thumbLoc, &font, CV_RGB(255, 0, 0));
					if(index >= turnNumber && curTime-startTurn > turnNumber*500 + 2500)
					{
						simonsTurn = false;
						index = 0;
					}
				}
				
			}
			else if(nframes > nframesToLearnBG && !fail)
			{
				
				if(index > turnNumber && turnNumber > -1)
				{
					cvPutText(rawImage, "GOOD JOB!", thumbLoc, &font, CV_RGB(0, 0, 255));
					simonsTurn  = true;
					turnNumber++;
					index = 0;
					startTurn = clock();
				}
				else if(turnNumber > -1)
				{
					cvPutText(rawImage, "Your turn", thumbLoc, &font, CV_RGB(255, 0, 0));
					if(prevNumFingers == list[index] && curTime - lockTimer > 500)
					{
						printf("GOOD!\n");
						index++;
					}
					else
					{
						if(prevNumFingers != list[index - 1] && prevNumFingers != 0 && curTime - lockTimer > 500)
						{
							//game over
							fail = true;
						}
					}
				}
				else
				{
					simonsTurn  = true;
					index = 0;
					turnNumber++;
					startTurn = clock();
				}
			}
			else if(!fail)
			{
				cvPutText(rawImage, "Calibrating...", thumbLoc, &font, CV_RGB(0, 0, 255));
			}
			else
			{
				cvPutText(rawImage, "You Lose! :(", thumbLoc, &font, CV_RGB(0, 0, 255));
			}


		//	cvPutText(rawImage, "Fingers", fingerTextLoc, &font, CV_RGB(255, 255, 255));
		//	cvPutText(rawImage, itoa(prevNumFingers, text, 1), fingerLoc, &font, CV_RGB(255, 255, 255));

		//	cvFlip(ImaskCodeBookCC, ImaskCodeBookCC, 1);
            cvShowImage( "Raw", rawImage );
            cvShowImage( "ForegroundCodeBook",ImaskCodeBook);
            cvShowImage( "CodeBook_ConnectComp",ImaskCodeBookCC);
		}

        // User input:
        c = cvWaitKey(10)&0xFF;
        c = tolower(c);
        // End processing on ESC, q or Q
        if(c == 27 || c == 'q')
            break;
        //Else check for user input
        switch( c )
        {
        case 'h':
            help();
            break;
        case 'p':
            pause = !pause;
            break;
        case 's':
            singlestep = !singlestep;
            pause = false;
            break;
        case 'r':
            pause = false;
            singlestep = false;
            break;
        case ' ':
            cvBGCodeBookClearStale( model, 0 );
            nframes = 0;
            break;
            //CODEBOOK PARAMS
        case 'y': case '0':
        case 'u': case '1':
        case 'v': case '2':
        case 'a': case '3':
        case 'b': 
            ch[0] = c == 'y' || c == '0' || c == 'a' || c == '3';
            ch[1] = c == 'u' || c == '1' || c == 'a' || c == '3' || c == 'b';
            ch[2] = c == 'v' || c == '2' || c == 'a' || c == '3' || c == 'b';
            printf("CodeBook YUV Channels active: %d, %d, %d\n", ch[0], ch[1], ch[2] );
            break;
        case 'i': //modify max classification bounds (max bound goes higher)
        case 'o': //modify max classification bounds (max bound goes lower)
        case 'k': //modify min classification bounds (min bound goes lower)
        case 'l': //modify min classification bounds (min bound goes higher)
            {
            uchar* ptr = c == 'i' || c == 'o' ? model->modMax : model->modMin;
            for(n=0; n<NCHANNELS; n++)
            {
                if( ch[n] )
                {
                    int v = ptr[n] + (c == 'i' || c == 'l' ? 1 : -1);
                    ptr[n] = CV_CAST_8U(v);
                }
                printf("%d,", ptr[n]);
            }
            printf(" CodeBook %s Side\n", c == 'i' || c == 'o' ? "High" : "Low" );
            }
            break;
        }
    }		
    
    cvReleaseCapture( &capture );
    cvDestroyWindow( "Raw" );
    cvDestroyWindow( "ForegroundCodeBook");
    cvDestroyWindow( "CodeBook_ConnectComp");
    return 0;
}
